# 黑色边缘路径搜索
import cv2 as cv
import numpy as np
import math
import readline

def otsuThreshould(img):
    # 寻找黑色边缘
    imgGray = cv.cvtColor(img,cv.COLOR_RGB2GRAY)
    imgFiltered = cv.GaussianBlur(imgGray,(5,5),0)
    imgBin = cv.adaptiveThreshold(imgFiltered,255,cv.ADAPTIVE_THRESH_GAUSSIAN_C,cv.THRESH_BINARY_INV,101,15)
    
    kernel1 = np.ones((7,7))
    kernel2 = np.ones((7,7))
    kernel3 = np.ones((10,10))
    imgFiltered = cv.morphologyEx(imgBin,cv.MORPH_OPEN,kernel2)
    imgFiltered = cv.morphologyEx(imgFiltered,cv.MORPH_CLOSE,kernel1)
    # imgFiltered = cv.morphologyEx(imgFiltered,cv.MORPH_OPEN,kernel3)
    return imgFiltered

def relativeAngle(p1,p0):
    # 计算相对角度
    return math.atan2(p1[1]-p0[1],p1[0]-p0[0])
def relativeDistance(p1,p0):
    # 计算相对距离
    return ((p1[1]-p0[1])**2 + (p1[0]-p0[0])**2)**0.5

def searchInnerOuterContour(img):
    # 寻找内边缘和其对应的外边缘
    contours,hierarchy = cv.findContours(img,cv.RETR_CCOMP, cv.CHAIN_APPROX_SIMPLE)
    # 先找到一个不隶属于任何多边形的最大的多边形
    maxIndepContourIndex = 0
    maxIndepContourArea = 0
    for i in range(len(contours)):
        area = cv.contourArea(contours[i])
        if area > maxIndepContourArea and (hierarchy[0][i][3]==-1):
            maxIndepContourArea = area
            maxIndepContourIndex = i
    # 由这个多边形去找内多边形，也就是First Child
    outerContour = contours[maxIndepContourIndex]
    firstChildIndex = hierarchy[0][maxIndepContourIndex][2]
    innerContour = contours[firstChildIndex]
    return innerContour,outerContour

def generateTrackingPath(innerContour,outerContour):
    # 既然已经知道了内外的轮廓，就可以把内外的追踪点位提取出来
    # 如果是按照顺时针的顺序可以取一个中心点，然后atan2计算角度，按角度从大到小排序

    outerArea = cv.contourArea(outerContour)
    outerPerimeter = cv.arcLength(outerContour,True)
    outerPolyDP = cv.approxPolyDP(outerContour,0.02*outerPerimeter,True)
    npoints = len(outerPolyDP)
    outerPolyDP = outerPolyDP.reshape([npoints,2])
    
    innerArea = cv.contourArea(innerContour)
    innerPerimeter = cv.arcLength(innerContour,True)
    innerPolyDP = cv.approxPolyDP(innerContour,0.02*innerPerimeter,True)
    innerPolyDP = innerPolyDP.reshape([len(innerPolyDP),2])
    # 中心点
    centerPoint = outerPolyDP.T.sum(1) / npoints

    # 遍历所有外部点位，然后把外部点位所对应的最近的内部点位找出来
    # 稍显复杂但是是最为稳妥的做法
    trackPoints = []
    for i in range(npoints):
        minRelativeDistance = 10000
        nearestInnerPoint = 0
        for j in range(len(innerPolyDP)):
            distance = relativeDistance(outerPolyDP[i],innerPolyDP[j])
            if distance < minRelativeDistance:
                minRelativeDistance = distance
                nearestInnerPoint = j
        # 然后求与最近点的中心即可
        trackPoints.append((outerPolyDP[i] + innerPolyDP[nearestInnerPoint])/2.0)
    
    # 最后对所有点位做一次排序，角度从小到大排列即可（因为是顺时针）
    trackWithRelativeAngles = []
    for i in range(npoints):
        trackWithRelativeAngles.append((trackPoints[i],relativeAngle(trackPoints[i],centerPoint)))
    trackWithRelativeAngles = sorted(trackWithRelativeAngles,key=lambda x:x[1])
    return trackWithRelativeAngles